﻿using Modbus.Device;
using Modbus.Serial;
using System;
using System.Collections.Generic;
using System.IO.Ports;
using System.Linq;
using System.Runtime.Versioning;
using System.Text;
using System.Threading.Tasks;

namespace QbotAGV
{
    [SupportedOSPlatform("windows")]
    [SupportedOSPlatform("linux")]
    class KincoAxis // 导程10mm，电机转360度丝杆运动10mm
    {
        public IModbusSerialMaster master { get; set; }
        public SerialPort serial { get; private set; }
        public SerialPortAdapter adapter { get; private set; }
        public byte ModbusId { get; set; }
        public class ModbusAddress
        {
            public ushort TargetSpeed { get; set; } = 0x6F00;
            public ushort StatusWord { get; set; } = 0x3200;
            public ushort ControlWord { get; set; } = 0x3100;
            public ushort ActualCurrent { get; set; } = 0x3E00;
            public ushort ActualPos { get; set; } = 0x3700;
        }
        public class ModbusValue
        {
            public const ushort PowerOn = 0x000F;
            public const ushort PowerOff = 0x0006;
            public const double CurrentRatio = 42.6615969581749; //内部16进制和真实电流的换算关系，在手册上没找到，试出来的
            public ushort Status { get; set; } = 0;
            public int CmdVel { get; set; } = 0; //设定的速度[rpm]
            public int MaxVel { get; set; } = 500;
            public int ActualPos { get; set; } = 0;
            public double AcutalCurrent { get; set; } = 0;
        }
        public static ModbusAddress Adrress { get; set; } = new ModbusAddress();
        public ModbusValue ServoValue { get; set; } = new ModbusValue();
        public bool ModbusOnline
        {
            get
            {
                return serial.IsOpen & !(master is null) ;
            }
        }

        public KincoAxis(string com, int modbus_id)
        {
            serial = new SerialPort(com) { 
                BaudRate = 115200,
                ReadTimeout = 100
            };
            adapter = new SerialPortAdapter(serial);
            ModbusId = (byte)modbus_id;
            master = ModbusSerialMaster.CreateRtu(adapter);
        }

        public void Begin()
        {
            try
            {
                serial.Open();
                // create modbus master
                ushort[] registers = master.ReadHoldingRegisters(ModbusId, Adrress.StatusWord, 1);
                ServoValue.Status = registers[0];
            }
            catch (Exception)
            {
                serial.Close();
                throw;
            }
        }

        public bool SetVeloctiy(int vel)
        {
            try
            {
                if (Math.Abs(vel) > ServoValue.MaxVel)
                    throw new Exception($"Vel: {vel} Exceed Maxvel: {ServoValue.MaxVel}");
                ServoValue.CmdVel = vel;
                var servo_cmd_vel = (int)(vel * 512 * 10000.0 / 1875);
                ushort[] registers = new ushort[2] { LowWord((uint)servo_cmd_vel), HighWord((uint)servo_cmd_vel) };
                master.WriteMultipleRegisters(ModbusId, Adrress.TargetSpeed, registers);
                return true;
            }
            catch (Exception ex)
            {
                Console.WriteLine($"Set Velocity: {vel}, Error: {ex.Message}");
                return false;
            }
        }

        public bool Enable(bool on_off)
        {
            ushort cmd = on_off ? ModbusValue.PowerOn : ModbusValue.PowerOff;
            try
            {
                master.WriteSingleRegister(ModbusId, Adrress.ControlWord, cmd);
                return true;
            }
            catch (Exception ex)
            {
                Console.WriteLine($"Enalbe Axis {on_off}, Error: {ex.Message}");
                return false;
            }
        }

        private int GetInt32Value(ushort address)
        {
            try
            {
                int ret = 0;
                ushort[] registers = master.ReadHoldingRegisters(ModbusId, address, 2);
                byte[] bytes_low = BitConverter.GetBytes(registers[0]);
                byte[] bytes_high = BitConverter.GetBytes(registers[1]);
                byte[] bytes = new byte[4] {
                    bytes_low[0],
                    bytes_low[1],
                    bytes_high[0],
                    bytes_high[1]
                };
                ret = BitConverter.ToInt32(bytes);
                return ret;
            }
            catch (Exception ex)
            {
                throw new Exception($"Get Address {address.ToString("X4")} Value Error: {ex.Message}");
            }
        }

        public bool GetActualPos()
        {
            try
            {
                ServoValue.ActualPos = GetInt32Value(Adrress.ActualPos);
                return true;
            }
            catch (Exception ex)
            {
                Console.WriteLine($"Get Actual Postion Error: {ex.Message}");
                return false;
            }
        }

        public bool GetActualCurrent()
        {
            try
            {
                ushort[] registers = master.ReadHoldingRegisters(ModbusId, Adrress.ActualCurrent, 1);
                ServoValue.AcutalCurrent = (short)registers[0] / ModbusValue.CurrentRatio;
                return true;
            }
            catch (Exception ex)
            {
                Console.WriteLine($"Get Actual Current Error: {ex.Message}");
                return false;
            }
        }

        public void Dispose()
        {
            serial.Close();
            serial.Dispose();
            master.Dispose();
        }

        public static ushort LowWord(uint value)
        {
            var ret = value & 0xFFFF;
            return (ushort)ret;
        }

        public static ushort HighWord(uint value)
        {
            var ret = (value & 0xFFFF0000) >> 16;
            return (ushort)ret;
        }
    }
}
